/**
   @copyright (C) 2017 Melexis N.V.

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

*/
#include "MLX90640_I2C_Driver.h"
#include "MLX90640_API.h"
#include <math.h>

#include <Arduino.h>

void ExtractVDDParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractPTATParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractGainParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractTgcParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractResolutionParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractKsTaParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractKsToParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractAlphaParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractOffsetParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractKtaPixelParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractKvPixelParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractCPParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractCILCParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
int ExtractDeviatingPixels(uint16_t *eeData, paramsMLX90640 *mlx90640);
int CheckAdjacentPixels(uint16_t pix1, uint16_t pix2);
int CheckEEPROMValid(uint16_t *eeData);

int MLX90640_DumpEE(uint8_t slaveAddr, uint16_t *eeData) {
    return MLX90640_I2CRead(slaveAddr, 0x2400, 832, eeData);
}

int MLX90640_GetFrameData(uint8_t slaveAddr, uint16_t *frameData) {
    uint16_t dataReady = 1;
    uint16_t controlRegister1;
    uint16_t statusRegister;
    int error   = 1;
    uint8_t cnt = 0;

    // Wait for new data to be available
    dataReady = 0;
    while (dataReady == 0) {
        error = MLX90640_I2CRead(slaveAddr, 0x8000, 1, &statusRegister);
        if (error != 0) {
            return error;
        }
        dataReady = statusRegister & 0x0008;
    }

    while (dataReady != 0 && cnt < 5) {
        // Set bits 4 and 5: Start measurement and enable overwrite
        error = MLX90640_I2CWrite(slaveAddr, 0x8000, 0x0030);
        if (error == -1) {
            return error;
        }

        // Read 832 pixels
        error = MLX90640_I2CRead(slaveAddr, 0x0400, 832, frameData);
        if (error != 0) {
            return error;
        }

        // Check if dataReady bit has been set
        error = MLX90640_I2CRead(slaveAddr, 0x8000, 1, &statusRegister);
        if (error != 0) {
            return error;
        }
        dataReady = statusRegister & 0x0008;
        cnt       = cnt + 1;
    }

    if (cnt > 4) {
        return -8;
    }

    // Read the control register
    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);

    // Append the control register and the last subpage to the end of the frame
    // data array
    frameData[832] = controlRegister1;
    frameData[833] = statusRegister & 0x0007;  // Last measured subpage

    if (error != 0) {
        return error;
    }

    return frameData[833];  // Return the subpage # that this frame belongs to
}

int MLX90640_ExtractParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    int error = CheckEEPROMValid(eeData);

    if (error == 0) {
        ExtractVDDParameters(eeData, mlx90640);
        ExtractPTATParameters(eeData, mlx90640);
        ExtractGainParameters(eeData, mlx90640);
        ExtractTgcParameters(eeData, mlx90640);
        ExtractResolutionParameters(eeData, mlx90640);
        ExtractKsTaParameters(eeData, mlx90640);
        ExtractKsToParameters(eeData, mlx90640);
        ExtractAlphaParameters(eeData, mlx90640);
        ExtractOffsetParameters(eeData, mlx90640);
        ExtractKtaPixelParameters(eeData, mlx90640);
        ExtractKvPixelParameters(eeData, mlx90640);
        ExtractCPParameters(eeData, mlx90640);
        ExtractCILCParameters(eeData, mlx90640);
        error = ExtractDeviatingPixels(eeData, mlx90640);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640_SetResolution(uint8_t slaveAddr, uint8_t resolution) {
    uint16_t controlRegister1;
    int value;
    int error;

    value = (resolution & 0x03) << 10;

    // Read the current register
    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);

    if (error == 0) {
        // Clear bits 10 and 11
        value = (controlRegister1 & 0xF3FF) | value;
        error = MLX90640_I2CWrite(slaveAddr, 0x800D, value);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640_GetCurResolution(uint8_t slaveAddr) {
    uint16_t controlRegister1;
    int resolutionRAM;
    int error;

    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);
    if (error != 0) {
        return error;
    }
    resolutionRAM = (controlRegister1 & 0x0C00) >> 10;

    return resolutionRAM;
}

//------------------------------------------------------------------------------

int MLX90640_SetRefreshRate(uint8_t slaveAddr, uint8_t refreshRate) {
    uint16_t controlRegister1;
    int value;
    int error;

    value = (refreshRate & 0x07) << 7;

    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);
    if (error == 0) {
        value = (controlRegister1 & 0xFC7F) | value;
        error = MLX90640_I2CWrite(slaveAddr, 0x800D, value);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640_GetRefreshRate(uint8_t slaveAddr) {
    uint16_t controlRegister1;
    int refreshRate;
    int error;

    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);
    if (error != 0) {
        return error;
    }
    refreshRate = (controlRegister1 & 0x0380) >> 7;

    return refreshRate;
}

//------------------------------------------------------------------------------

int MLX90640_SetInterleavedMode(uint8_t slaveAddr) {
    uint16_t controlRegister1;
    int value;
    int error;

    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);

    if (error == 0) {
        value = (controlRegister1 & 0xEFFF);
        error = MLX90640_I2CWrite(slaveAddr, 0x800D, value);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640_SetChessMode(uint8_t slaveAddr) {
    uint16_t controlRegister1;
    int value;
    int error;

    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);

    if (error == 0) {
        value = (controlRegister1 | 0x1000);
        error = MLX90640_I2CWrite(slaveAddr, 0x800D, value);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640_GetCurMode(uint8_t slaveAddr) {
    uint16_t controlRegister1;
    int modeRAM;
    int error;

    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);
    if (error != 0) {
        return error;
    }
    modeRAM = (controlRegister1 & 0x1000) >> 12;

    return modeRAM;
}

//------------------------------------------------------------------------------

void MLX90640_CalculateTo(uint16_t *frameData, paramsMLX90640 *params,
                          float emissivity, float tr, float *result) {
    float vdd;
    float ta;
    float ta4;
    float tr4;
    float taTr;
    float gain;
    float irDataCP[2];
    float irData;
    float alphaCompensated;
    uint8_t mode;
    int8_t ilPattern;
    int8_t chessPattern;
    int8_t pattern;
    int8_t conversionPattern;
    float Sx;
    float To;
    float alphaCorrR[4];
    int8_t range;

    vdd  = MLX90640_GetVdd(frameData, params);
    ta   = MLX90640_GetTa(frameData, params);
    ta4  = pow((ta + 273.15), (double)4);
    tr4  = pow((tr + 273.15), (double)4);
    taTr = tr4 - (tr4 - ta4) / emissivity;

    alphaCorrR[0] = 1 / (1 + params->ksTo[0] * 40);
    alphaCorrR[1] = 1;
    alphaCorrR[2] = (1 + params->ksTo[2] * params->ct[2]);
    alphaCorrR[3] =
        alphaCorrR[2] * (1 + params->ksTo[3] * (params->ct[3] - params->ct[2]));

    //------------------------- Gain calculation
    //-----------------------------------
    gain = frameData[778];
    if (gain > 32767) {
        gain = gain - 65536;
    }

    gain = params->gainEE / gain;

    //------------------------- To calculation
    //-------------------------------------
    mode = (frameData[832] & 0x1000) >> 5;

    irDataCP[0] = frameData[776];
    irDataCP[1] = frameData[808];
    for (int i = 0; i < 2; i++) {
        if (irDataCP[i] > 32767) {
            irDataCP[i] = irDataCP[i] - 65536;
        }
        irDataCP[i] = irDataCP[i] * gain;
    }
    irDataCP[0] = irDataCP[0] - params->cpOffset[0] *
                                    (1 + params->cpKta * (ta - 25)) *
                                    (1 + params->cpKv * (vdd - 3.3));
    if (mode == params->calibrationModeEE) {
        irDataCP[1] = irDataCP[1] - params->cpOffset[1] *
                                        (1 + params->cpKta * (ta - 25)) *
                                        (1 + params->cpKv * (vdd - 3.3));
    } else {
        irDataCP[1] =
            irDataCP[1] - (params->cpOffset[1] + params->ilChessC[0]) *
                              (1 + params->cpKta * (ta - 25)) *
                              (1 + params->cpKv * (vdd - 3.3));
    }

    for (int pixelNumber = 0; pixelNumber < 768; pixelNumber++) {
        ilPattern =
            int((pixelNumber) / 32) - int(int((pixelNumber) / 32) / 2) * 2;
        chessPattern = ilPattern ^ (pixelNumber - int(pixelNumber / 2) * 2);
        conversionPattern =
            (int((pixelNumber - 2) / 4) - int((pixelNumber - 1) / 4) +
             int((pixelNumber + 1) / 4) - int((pixelNumber) / 4)) *
            (1 - 2 * ilPattern);

        if (mode == 0) {
            pattern = ilPattern;
        } else {
            pattern = chessPattern;
        }

        if (pattern == frameData[833]) {
            irData = frameData[pixelNumber];
            if (irData > 32767) {
                irData = irData - 65536;
            }
            irData = irData * gain;

            irData = irData - params->offset[pixelNumber] *
                                  (1 + params->kta[pixelNumber] * (ta - 25)) *
                                  (1 + params->kv[pixelNumber] * (vdd - 3.3));
            if (mode != params->calibrationModeEE) {
                irData = irData + params->ilChessC[2] * (2 * ilPattern - 1) -
                         params->ilChessC[1] * conversionPattern;
            }

            irData = irData / emissivity;

            irData = irData - params->tgc * ((1 - pattern) * irDataCP[0] +
                                             pattern * irDataCP[1]);

            alphaCompensated =
                (params->alpha[pixelNumber] -
                 params->tgc * ((1 - pattern) * params->cpAlpha[0] +
                                pattern * params->cpAlpha[1])) *
                (1 + params->KsTa * (ta - 25));

            // Sx = pow((double)alphaCompensated, (double)3) * (irData +
            // alphaCompensated * taTr);
            Sx = (double)alphaCompensated * alphaCompensated *
                 alphaCompensated *
                 (irData +
                  alphaCompensated * taTr);  // improved from 5fps to 7 fps
            Sx = sqrt(sqrt(Sx)) * params->ksTo[1];

            To = sqrt(sqrt(irData / (alphaCompensated *
                                         (1 - params->ksTo[1] * 273.15) +
                                     Sx) +
                           taTr)) -
                 273.15;

            if (To < params->ct[1]) {
                range = 0;
            } else if (To < params->ct[2]) {
                range = 1;
            } else if (To < params->ct[3]) {
                range = 2;
            } else {
                range = 3;
            }

            To = sqrt(sqrt(irData / (alphaCompensated * alphaCorrR[range] *
                                     (1 + params->ksTo[range] *
                                              (To - params->ct[range]))) +
                           taTr)) -
                 273.15;

            result[pixelNumber] = To;
        }
    }
}

//------------------------------------------------------------------------------

void MLX90640_GetImage(uint16_t *frameData, paramsMLX90640 *params,
                       float *result) {
    float vdd;
    float ta;
    float gain;
    float irDataCP[2];
    float irData;
    float alphaCompensated;
    uint8_t mode;
    int8_t ilPattern;
    int8_t chessPattern;
    int8_t pattern;
    int8_t conversionPattern;
    float image;

    vdd = MLX90640_GetVdd(frameData, params);
    ta  = MLX90640_GetTa(frameData, params);

    //------------------------- Gain calculation
    //-----------------------------------
    gain = frameData[778];
    if (gain > 32767) {
        gain = gain - 65536;
    }

    gain = params->gainEE / gain;

    //------------------------- Image calculation
    //-------------------------------------
    mode = (frameData[832] & 0x1000) >> 5;

    irDataCP[0] = frameData[776];
    irDataCP[1] = frameData[808];
    for (int i = 0; i < 2; i++) {
        if (irDataCP[i] > 32767) {
            irDataCP[i] = irDataCP[i] - 65536;
        }
        irDataCP[i] = irDataCP[i] * gain;
    }
    irDataCP[0] = irDataCP[0] - params->cpOffset[0] *
                                    (1 + params->cpKta * (ta - 25)) *
                                    (1 + params->cpKv * (vdd - 3.3));
    if (mode == params->calibrationModeEE) {
        irDataCP[1] = irDataCP[1] - params->cpOffset[1] *
                                        (1 + params->cpKta * (ta - 25)) *
                                        (1 + params->cpKv * (vdd - 3.3));
    } else {
        irDataCP[1] =
            irDataCP[1] - (params->cpOffset[1] + params->ilChessC[0]) *
                              (1 + params->cpKta * (ta - 25)) *
                              (1 + params->cpKv * (vdd - 3.3));
    }

    for (int pixelNumber = 0; pixelNumber < 768; pixelNumber++) {
        ilPattern =
            int((pixelNumber) / 32) - int(int((pixelNumber) / 32) / 2) * 2;
        chessPattern = ilPattern ^ (pixelNumber - int(pixelNumber / 2) * 2);
        conversionPattern =
            (int((pixelNumber - 2) / 4) - int((pixelNumber - 1) / 4) +
             int((pixelNumber + 1) / 4) - int((pixelNumber) / 4)) *
            (1 - 2 * ilPattern);

        if (mode == 0) {
            pattern = ilPattern;
        } else {
            pattern = chessPattern;
        }

        if (pattern == frameData[833]) {
            irData = frameData[pixelNumber];
            if (irData > 32767) {
                irData = irData - 65536;
            }
            irData = irData * gain;

            irData = irData - params->offset[pixelNumber] *
                                  (1 + params->kta[pixelNumber] * (ta - 25)) *
                                  (1 + params->kv[pixelNumber] * (vdd - 3.3));
            if (mode != params->calibrationModeEE) {
                irData = irData + params->ilChessC[2] * (2 * ilPattern - 1) -
                         params->ilChessC[1] * conversionPattern;
            }

            irData = irData - params->tgc * ((1 - pattern) * irDataCP[0] +
                                             pattern * irDataCP[1]);

            alphaCompensated =
                (params->alpha[pixelNumber] -
                 params->tgc * ((1 - pattern) * params->cpAlpha[0] +
                                pattern * params->cpAlpha[1]));

            image = irData / alphaCompensated;

            result[pixelNumber] = image;
        }
    }
}

//------------------------------------------------------------------------------

float MLX90640_GetVdd(uint16_t *frameData, paramsMLX90640 *params) {
    float resolutionCorrection;
    int resolutionRAM;
    float vdd = frameData[810];
    if (vdd > 32767) {
        vdd = vdd - 65536;
    }
    resolutionRAM = (frameData[832] & 0x0C00) >> 10;
    resolutionCorrection =
        pow(2, (double)params->resolutionEE) / pow(2, (double)resolutionRAM);
    vdd = (resolutionCorrection * vdd - params->vdd25) / params->kVdd + 3.3;

    return vdd;
}

void ExtractVDDParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    int16_t kVdd;
    int16_t vdd25;

    // eeData[51] = 0x9D68; //Test data - should produce a VCC of 3.319

    kVdd = (eeData[51] & 0xFF00) >> 8;
    if (kVdd > 127) {
        kVdd = kVdd - 256;
    }
    kVdd = 32 * kVdd;

    vdd25 = eeData[51] & 0x00FF;
    vdd25 = ((vdd25 - 256) << 5) - 8192;

    mlx90640->kVdd  = kVdd;
    mlx90640->vdd25 = vdd25;
}

//------------------------------------------------------------------------------

float MLX90640_GetTa(uint16_t *frameData, paramsMLX90640 *params) {
    float ptat;
    float ptatArt;
    float vdd;
    float ta;

    vdd = MLX90640_GetVdd(frameData, params);

    ptat = frameData[800];
    if (ptat > 32767) {
        ptat = ptat - 65536;
    }

    ptatArt = frameData[768];
    if (ptatArt > 32767) {
        ptatArt = ptatArt - 65536;
    }
    ptatArt =
        (ptat / (ptat * params->alphaPTAT + ptatArt)) * pow(2, (double)18);

    ta = (ptatArt / (1 + params->KvPTAT * (vdd - 3.3)) - params->vPTAT25);
    ta = ta / params->KtPTAT + 25;

    return ta;
}

//------------------------------------------------------------------------------

int MLX90640_GetSubPageNumber(uint16_t *frameData) {
    return frameData[833];
}

//------------------------------------------------------------------------------
/*
  void ExtractVDDParameters(uint16_t *eeData, paramsMLX90640 *mlx90640)
  {
  int16_t kVdd;
  int16_t vdd25;

  //myData[0] = 0x9D68; //Test data - should produce a VCC of 3.319

  kVdd = eeData[51];
  kVdd = (eeData[51] & 0xFF00) >> 8;
  if (kVdd > 127)
  {
    kVdd = kVdd - 256;
  }
  kVdd = 32 * kVdd;

  eeData[51] = 0x9D68; //Test data

  vdd25 = eeData[51] & 0x00FF;
  vdd25 = ((vdd25 - 256) << 5) - 8192;
  Serial.print("vdd25 from params: ");
  Serial.println(vdd25);

  mlx90640->kVdd = kVdd;
  mlx90640->vdd25 = vdd25;
  }
*/
//------------------------------------------------------------------------------

void ExtractPTATParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    float KvPTAT;
    float KtPTAT;
    int16_t vPTAT25;
    int16_t alphaPTAT;

    KvPTAT = (eeData[50] & 0xFC00) >> 10;
    if (KvPTAT > 31) {
        KvPTAT = KvPTAT - 64;
    }
    KvPTAT = KvPTAT / 4096;

    KtPTAT = eeData[50] & 0x03FF;
    if (KtPTAT > 511) {
        KtPTAT = KtPTAT - 1024;
    }
    KtPTAT = KtPTAT / 8;

    vPTAT25 = eeData[49];

    alphaPTAT = ((eeData[16] & 0xF000) >> 14) + 8;

    mlx90640->KvPTAT    = KvPTAT;
    mlx90640->KtPTAT    = KtPTAT;
    mlx90640->vPTAT25   = vPTAT25;
    mlx90640->alphaPTAT = alphaPTAT;
}

//------------------------------------------------------------------------------

void ExtractGainParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    int16_t gainEE;

    gainEE = eeData[48];
    if (gainEE > 32767) {
        gainEE = gainEE - 65536;
    }

    mlx90640->gainEE = gainEE;
}

//------------------------------------------------------------------------------

void ExtractTgcParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    float tgc;
    tgc = eeData[60] & 0x00FF;
    if (tgc > 127) {
        tgc = tgc - 256;
    }
    tgc = tgc / 32.0f;

    mlx90640->tgc = tgc;
}

//------------------------------------------------------------------------------

void ExtractResolutionParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    uint8_t resolutionEE;
    resolutionEE = (eeData[56] & 0x3000) >> 12;

    mlx90640->resolutionEE = resolutionEE;
}

//------------------------------------------------------------------------------

void ExtractKsTaParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    float KsTa;
    KsTa = (eeData[60] & 0xFF00) >> 8;
    if (KsTa > 127) {
        KsTa = KsTa - 256;
    }
    KsTa = KsTa / 8192.0f;

    mlx90640->KsTa = KsTa;
}

//------------------------------------------------------------------------------

void ExtractKsToParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    int KsToScale;
    int8_t step;

    step = ((eeData[63] & 0x3000) >> 12) * 10;

    mlx90640->ct[0] = -40;
    mlx90640->ct[1] = 0;
    mlx90640->ct[2] = (eeData[63] & 0x00F0) >> 4;
    mlx90640->ct[3] = (eeData[63] & 0x0F00) >> 8;

    mlx90640->ct[2] = mlx90640->ct[2] * step;
    mlx90640->ct[3] = mlx90640->ct[2] + mlx90640->ct[3] * step;

    KsToScale = (eeData[63] & 0x000F) + 8;
    KsToScale = 1 << KsToScale;

    mlx90640->ksTo[0] = eeData[61] & 0x00FF;
    mlx90640->ksTo[1] = (eeData[61] & 0xFF00) >> 8;
    mlx90640->ksTo[2] = eeData[62] & 0x00FF;
    mlx90640->ksTo[3] = (eeData[62] & 0xFF00) >> 8;

    for (int i = 0; i < 4; i++) {
        if (mlx90640->ksTo[i] > 127) {
            mlx90640->ksTo[i] = mlx90640->ksTo[i] - 256;
        }
        mlx90640->ksTo[i] = mlx90640->ksTo[i] / KsToScale;
    }
}

//------------------------------------------------------------------------------

void ExtractAlphaParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    int accRow[24];
    int accColumn[32];
    int p = 0;
    int alphaRef;
    uint8_t alphaScale;
    uint8_t accRowScale;
    uint8_t accColumnScale;
    uint8_t accRemScale;

    accRemScale    = eeData[32] & 0x000F;
    accColumnScale = (eeData[32] & 0x00F0) >> 4;
    accRowScale    = (eeData[32] & 0x0F00) >> 8;
    alphaScale     = ((eeData[32] & 0xF000) >> 12) + 30;
    alphaRef       = eeData[33];

    for (int i = 0; i < 6; i++) {
        p             = i * 4;
        accRow[p + 0] = (eeData[34 + i] & 0x000F);
        accRow[p + 1] = (eeData[34 + i] & 0x00F0) >> 4;
        accRow[p + 2] = (eeData[34 + i] & 0x0F00) >> 8;
        accRow[p + 3] = (eeData[34 + i] & 0xF000) >> 12;
    }

    for (int i = 0; i < 24; i++) {
        if (accRow[i] > 7) {
            accRow[i] = accRow[i] - 16;
        }
    }

    for (int i = 0; i < 8; i++) {
        p                = i * 4;
        accColumn[p + 0] = (eeData[40 + i] & 0x000F);
        accColumn[p + 1] = (eeData[40 + i] & 0x00F0) >> 4;
        accColumn[p + 2] = (eeData[40 + i] & 0x0F00) >> 8;
        accColumn[p + 3] = (eeData[40 + i] & 0xF000) >> 12;
    }

    for (int i = 0; i < 32; i++) {
        if (accColumn[i] > 7) {
            accColumn[i] = accColumn[i] - 16;
        }
    }

    for (int i = 0; i < 24; i++) {
        for (int j = 0; j < 32; j++) {
            p                  = 32 * i + j;
            mlx90640->alpha[p] = (eeData[64 + p] & 0x03F0) >> 4;
            if (mlx90640->alpha[p] > 31) {
                mlx90640->alpha[p] = mlx90640->alpha[p] - 64;
            }
            mlx90640->alpha[p] = mlx90640->alpha[p] * (1 << accRemScale);
            mlx90640->alpha[p] =
                (alphaRef + (accRow[i] << accRowScale) +
                 (accColumn[j] << accColumnScale) + mlx90640->alpha[p]);
            mlx90640->alpha[p] =
                mlx90640->alpha[p] / pow(2, (double)alphaScale);
        }
    }
}

//------------------------------------------------------------------------------

void ExtractOffsetParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    int occRow[24];
    int occColumn[32];
    int p = 0;
    int16_t offsetRef;
    uint8_t occRowScale;
    uint8_t occColumnScale;
    uint8_t occRemScale;

    occRemScale    = (eeData[16] & 0x000F);
    occColumnScale = (eeData[16] & 0x00F0) >> 4;
    occRowScale    = (eeData[16] & 0x0F00) >> 8;
    offsetRef      = eeData[17];
    if (offsetRef > 32767) {
        offsetRef = offsetRef - 65536;
    }

    for (int i = 0; i < 6; i++) {
        p             = i * 4;
        occRow[p + 0] = (eeData[18 + i] & 0x000F);
        occRow[p + 1] = (eeData[18 + i] & 0x00F0) >> 4;
        occRow[p + 2] = (eeData[18 + i] & 0x0F00) >> 8;
        occRow[p + 3] = (eeData[18 + i] & 0xF000) >> 12;
    }

    for (int i = 0; i < 24; i++) {
        if (occRow[i] > 7) {
            occRow[i] = occRow[i] - 16;
        }
    }

    for (int i = 0; i < 8; i++) {
        p                = i * 4;
        occColumn[p + 0] = (eeData[24 + i] & 0x000F);
        occColumn[p + 1] = (eeData[24 + i] & 0x00F0) >> 4;
        occColumn[p + 2] = (eeData[24 + i] & 0x0F00) >> 8;
        occColumn[p + 3] = (eeData[24 + i] & 0xF000) >> 12;
    }

    for (int i = 0; i < 32; i++) {
        if (occColumn[i] > 7) {
            occColumn[i] = occColumn[i] - 16;
        }
    }

    for (int i = 0; i < 24; i++) {
        for (int j = 0; j < 32; j++) {
            p                   = 32 * i + j;
            mlx90640->offset[p] = (eeData[64 + p] & 0xFC00) >> 10;
            if (mlx90640->offset[p] > 31) {
                mlx90640->offset[p] = mlx90640->offset[p] - 64;
            }
            mlx90640->offset[p] = mlx90640->offset[p] * (1 << occRemScale);
            mlx90640->offset[p] =
                (offsetRef + (occRow[i] << occRowScale) +
                 (occColumn[j] << occColumnScale) + mlx90640->offset[p]);
        }
    }
}

//------------------------------------------------------------------------------

void ExtractKtaPixelParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    int p = 0;
    int8_t KtaRC[4];
    int8_t KtaRoCo;
    int8_t KtaRoCe;
    int8_t KtaReCo;
    int8_t KtaReCe;
    uint8_t ktaScale1;
    uint8_t ktaScale2;
    uint8_t split;

    KtaRoCo = (eeData[54] & 0xFF00) >> 8;
    if (KtaRoCo > 127) {
        KtaRoCo = KtaRoCo - 256;
    }
    KtaRC[0] = KtaRoCo;

    KtaReCo = (eeData[54] & 0x00FF);
    if (KtaReCo > 127) {
        KtaReCo = KtaReCo - 256;
    }
    KtaRC[2] = KtaReCo;

    KtaRoCe = (eeData[55] & 0xFF00) >> 8;
    if (KtaRoCe > 127) {
        KtaRoCe = KtaRoCe - 256;
    }
    KtaRC[1] = KtaRoCe;

    KtaReCe = (eeData[55] & 0x00FF);
    if (KtaReCe > 127) {
        KtaReCe = KtaReCe - 256;
    }
    KtaRC[3] = KtaReCe;

    ktaScale1 = ((eeData[56] & 0x00F0) >> 4) + 8;
    ktaScale2 = (eeData[56] & 0x000F);

    for (int i = 0; i < 24; i++) {
        for (int j = 0; j < 32; j++) {
            p     = 32 * i + j;
            split = 2 * (int(p / 32) - int(int(p / 32) / 2) * 2) + p % 2;
            mlx90640->kta[p] = (eeData[64 + p] & 0x000E) >> 1;
            if (mlx90640->kta[p] > 3) {
                mlx90640->kta[p] = mlx90640->kta[p] - 8;
            }
            mlx90640->kta[p] = mlx90640->kta[p] * (1 << ktaScale2);
            mlx90640->kta[p] = KtaRC[split] + mlx90640->kta[p];
            mlx90640->kta[p] = mlx90640->kta[p] / pow(2, (double)ktaScale1);
        }
    }
}

//------------------------------------------------------------------------------

void ExtractKvPixelParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    int p = 0;
    int8_t KvT[4];
    int8_t KvRoCo;
    int8_t KvRoCe;
    int8_t KvReCo;
    int8_t KvReCe;
    uint8_t kvScale;
    uint8_t split;

    KvRoCo = (eeData[52] & 0xF000) >> 12;
    if (KvRoCo > 7) {
        KvRoCo = KvRoCo - 16;
    }
    KvT[0] = KvRoCo;

    KvReCo = (eeData[52] & 0x0F00) >> 8;
    if (KvReCo > 7) {
        KvReCo = KvReCo - 16;
    }
    KvT[2] = KvReCo;

    KvRoCe = (eeData[52] & 0x00F0) >> 4;
    if (KvRoCe > 7) {
        KvRoCe = KvRoCe - 16;
    }
    KvT[1] = KvRoCe;

    KvReCe = (eeData[52] & 0x000F);
    if (KvReCe > 7) {
        KvReCe = KvReCe - 16;
    }
    KvT[3] = KvReCe;

    kvScale = (eeData[56] & 0x0F00) >> 8;

    for (int i = 0; i < 24; i++) {
        for (int j = 0; j < 32; j++) {
            p     = 32 * i + j;
            split = 2 * (int(p / 32) - int(int(p / 32) / 2) * 2) + p % 2;
            mlx90640->kv[p] = KvT[split];
            mlx90640->kv[p] = mlx90640->kv[p] / pow(2, (double)kvScale);
        }
    }
}

//------------------------------------------------------------------------------

void ExtractCPParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    float alphaSP[2];
    int16_t offsetSP[2];
    float cpKv;
    float cpKta;
    uint8_t alphaScale;
    uint8_t ktaScale1;
    uint8_t kvScale;

    alphaScale = ((eeData[32] & 0xF000) >> 12) + 27;

    offsetSP[0] = (eeData[58] & 0x03FF);
    if (offsetSP[0] > 511) {
        offsetSP[0] = offsetSP[0] - 1024;
    }

    offsetSP[1] = (eeData[58] & 0xFC00) >> 10;
    if (offsetSP[1] > 31) {
        offsetSP[1] = offsetSP[1] - 64;
    }
    offsetSP[1] = offsetSP[1] + offsetSP[0];

    alphaSP[0] = (eeData[57] & 0x03FF);
    if (alphaSP[0] > 511) {
        alphaSP[0] = alphaSP[0] - 1024;
    }
    alphaSP[0] = alphaSP[0] / pow(2, (double)alphaScale);

    alphaSP[1] = (eeData[57] & 0xFC00) >> 10;
    if (alphaSP[1] > 31) {
        alphaSP[1] = alphaSP[1] - 64;
    }
    alphaSP[1] = (1 + alphaSP[1] / 128) * alphaSP[0];

    cpKta = (eeData[59] & 0x00FF);
    if (cpKta > 127) {
        cpKta = cpKta - 256;
    }
    ktaScale1       = ((eeData[56] & 0x00F0) >> 4) + 8;
    mlx90640->cpKta = cpKta / pow(2, (double)ktaScale1);

    cpKv = (eeData[59] & 0xFF00) >> 8;
    if (cpKv > 127) {
        cpKv = cpKv - 256;
    }
    kvScale        = (eeData[56] & 0x0F00) >> 8;
    mlx90640->cpKv = cpKv / pow(2, (double)kvScale);

    mlx90640->cpAlpha[0]  = alphaSP[0];
    mlx90640->cpAlpha[1]  = alphaSP[1];
    mlx90640->cpOffset[0] = offsetSP[0];
    mlx90640->cpOffset[1] = offsetSP[1];
}

//------------------------------------------------------------------------------

void ExtractCILCParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    float ilChessC[3];
    uint8_t calibrationModeEE;

    calibrationModeEE = (eeData[10] & 0x0800) >> 4;
    calibrationModeEE = calibrationModeEE ^ 0x80;

    ilChessC[0] = (eeData[53] & 0x003F);
    if (ilChessC[0] > 31) {
        ilChessC[0] = ilChessC[0] - 64;
    }
    ilChessC[0] = ilChessC[0] / 16.0f;

    ilChessC[1] = (eeData[53] & 0x07C0) >> 6;
    if (ilChessC[1] > 15) {
        ilChessC[1] = ilChessC[1] - 32;
    }
    ilChessC[1] = ilChessC[1] / 2.0f;

    ilChessC[2] = (eeData[53] & 0xF800) >> 11;
    if (ilChessC[2] > 15) {
        ilChessC[2] = ilChessC[2] - 32;
    }
    ilChessC[2] = ilChessC[2] / 8.0f;

    mlx90640->calibrationModeEE = calibrationModeEE;
    mlx90640->ilChessC[0]       = ilChessC[0];
    mlx90640->ilChessC[1]       = ilChessC[1];
    mlx90640->ilChessC[2]       = ilChessC[2];
}

//------------------------------------------------------------------------------

int ExtractDeviatingPixels(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    uint16_t pixCnt        = 0;
    uint16_t brokenPixCnt  = 0;
    uint16_t outlierPixCnt = 0;
    int warn               = 0;
    int i;

    for (pixCnt = 0; pixCnt < 5; pixCnt++) {
        mlx90640->brokenPixels[pixCnt]  = 0xFFFF;
        mlx90640->outlierPixels[pixCnt] = 0xFFFF;
    }

    pixCnt = 0;
    while (pixCnt < 768 && brokenPixCnt < 5 && outlierPixCnt < 5) {
        if (eeData[pixCnt + 64] == 0) {
            mlx90640->brokenPixels[brokenPixCnt] = pixCnt;
            brokenPixCnt                         = brokenPixCnt + 1;
        } else if ((eeData[pixCnt + 64] & 0x0001) != 0) {
            mlx90640->outlierPixels[outlierPixCnt] = pixCnt;
            outlierPixCnt                          = outlierPixCnt + 1;
        }

        pixCnt = pixCnt + 1;
    }

    if (brokenPixCnt > 4) {
        warn = -3;
    } else if (outlierPixCnt > 4) {
        warn = -4;
    } else if ((brokenPixCnt + outlierPixCnt) > 4) {
        warn = -5;
    } else {
        for (pixCnt = 0; pixCnt < brokenPixCnt; pixCnt++) {
            for (i = pixCnt + 1; i < brokenPixCnt; i++) {
                warn = CheckAdjacentPixels(mlx90640->brokenPixels[pixCnt],
                                           mlx90640->brokenPixels[i]);
                if (warn != 0) {
                    return warn;
                }
            }
        }

        for (pixCnt = 0; pixCnt < outlierPixCnt; pixCnt++) {
            for (i = pixCnt + 1; i < outlierPixCnt; i++) {
                warn = CheckAdjacentPixels(mlx90640->outlierPixels[pixCnt],
                                           mlx90640->outlierPixels[i]);
                if (warn != 0) {
                    return warn;
                }
            }
        }

        for (pixCnt = 0; pixCnt < brokenPixCnt; pixCnt++) {
            for (i = 0; i < outlierPixCnt; i++) {
                warn = CheckAdjacentPixels(mlx90640->brokenPixels[pixCnt],
                                           mlx90640->outlierPixels[i]);
                if (warn != 0) {
                    return warn;
                }
            }
        }
    }

    return warn;
}

//------------------------------------------------------------------------------

int CheckAdjacentPixels(uint16_t pix1, uint16_t pix2) {
    int pixPosDif;

    pixPosDif = pix1 - pix2;
    if (pixPosDif > -34 && pixPosDif < -30) {
        return -6;
    }
    if (pixPosDif > -2 && pixPosDif < 2) {
        return -6;
    }
    if (pixPosDif > 30 && pixPosDif < 34) {
        return -6;
    }

    return 0;
}

//------------------------------------------------------------------------------

int CheckEEPROMValid(uint16_t *eeData) {
    int deviceSelect;
    deviceSelect = eeData[10] & 0x0040;
    if (deviceSelect == 0) {
        return 0;
    }

    return -7;
}
